#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "driver/i2c.h"
#include "math.h"
#include "esp_timer.h"

#include "PosSensor.h"
#include "PAC9535Driver/PCA9535Driver.h"
#include "AD7997Driver/AD7997Driver.h"
#include "MotorCtrl/MotorCtrl.h"
#include "cmd/cmd.h"

#define PCA_LOW             0
#define PCA_HIGH            1

#define PosSenDownPin1      0
#define PosSenDownPin2      1
#define PosSenDownPin3      2
#define PosSenDownPin4      3
#define PosSenDownPin5      4
#define PosSenDownPin6      5
#define PosSenDownPin7      6
#define PosSenDownPin8      7

#define PosSenCtrlPin1      3
#define PosSenCtrlPin2      4
#define PosSenCtrlPin3      5
#define PosSenCtrlPin4      6
#define PosSenCtrlPin5      7
#define PosSenCtrlPin6      2
#define PosSenCtrlPin7      1
#define PosSenCtrlPin8      0

#define PosSenUpperCh1      (1 << 4)
#define PosSenUpperCh2      (1 << 6)
#define PosSenUpperCh3      (1 << 8)
#define PosSenUpperCh4      (1 << 10)
#define PosSenUpperCh5      (1 << 11)
#define PosSenUpperCh6      (1 << 9)
#define PosSenUpperCh7      (1 << 7)
#define PosSenUpperCh8      (1 << 5)
#define PosSenUpperChAll    (0x0ff0)

#define PosSenUpTranCh1     0x80
#define PosSenUpTranCh2     0x88
#define PosSenUpTranCh3     0x90
#define PosSenUpTranCh4     0x98
#define PosSenUpTranCh5     0xA0
#define PosSenUpTranCh6     0xA8
#define PosSenUpTranCh7     0xB0
#define PosSenUpTranCh8     0xB8

#define I2C_MASTER_SCL_IO          22      // GPIO引脚号
#define I2C_MASTER_SDA_IO          21      // GPIO引脚号
#define I2C_MASTER_FREQ_HZ         100000  // I2C主时钟频率
#define I2C_MASTER_PORT_NUM        I2C_NUM_0

#define BottomSensorTrigger        0

#define TrayHeight                 (6.7)


const uint16_t PosSenDownPin2RealPin[8] = {PosSenDownPin1, PosSenDownPin2, PosSenDownPin3, PosSenDownPin4, PosSenDownPin5, PosSenDownPin6, PosSenDownPin7, PosSenDownPin8};
const uint16_t PosSenCtrlPin2RealPin[8] = {PosSenCtrlPin1, PosSenCtrlPin2, PosSenCtrlPin3, PosSenCtrlPin4, PosSenCtrlPin5, PosSenCtrlPin6, PosSenCtrlPin7, PosSenCtrlPin8};
const uint16_t PosSenUpperCh2RealPin[8] = {PosSenUpperCh1, PosSenUpperCh2, PosSenUpperCh3, PosSenUpperCh4, PosSenUpperCh5, PosSenUpperCh6, PosSenUpperCh7, PosSenUpperCh8};
const uint16_t PosSenUpTran2RealPin[8] = {PosSenUpTranCh1, PosSenUpTranCh2, PosSenUpTranCh3, PosSenUpTranCh4, PosSenUpTranCh5, PosSenUpTranCh6, PosSenUpTranCh7, PosSenUpTranCh8};


QueueHandle_t MotorCodeQueue;
QueueHandle_t MotorHandCtrlQueue;
char *TAG = "PosSensor";
extern CmdHandle_t CmdHandle;
bool sleepFlag = 0;
uint8_t trayNum[AxisNum] = {0};
static double taryZeroPos[AxisNum] = {0.0};
// double currentMotorPos[AxisNum];
double currentMotorPos[9];
double lastMotorPos[AxisNum] = {0};

static void i2c_master_init() ;
static void SetTaryZeroPos(MotorAxis_t Axis);
static uint8_t GetTrayNum(MotorAxis_t Axis);

static void i2c_master_init() 
{
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_DISABLE,
        .scl_pullup_en = GPIO_PULLUP_DISABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };
    
    ESP_ERROR_CHECK(i2c_param_config(I2C_MASTER_PORT_NUM, &conf));
    ESP_ERROR_CHECK(i2c_driver_install(I2C_MASTER_PORT_NUM, conf.mode, 0, 0, 0));
}


uint16_t UpSensirTH = 900;

void setMotorSleep(void* n)
{
    sleepFlag = 1;
    PosHandCmd_t cmd = {MotorSleep, Axis1};
    xQueueSend(MotorHandCtrlQueue, &cmd, portMAX_DELAY);
}

void setMotorSearchBottom(void* n)
{
    PosHandCmd_t cmd = {HandSearchDownward, Axis1};
    xQueueSend(MotorHandCtrlQueue, &cmd, portMAX_DELAY);
}

void setMotorRun(void* n)
{
    PosHandCmd_t cmd = {RunState, Axis1};
    xQueueSend(MotorHandCtrlQueue, &cmd, portMAX_DELAY);
}

void getTaryNum(void* n)
{
    char buf[100] = {0};
    uint8_t offset = 0;
    for(uint8_t i = 0; i < AxisNum; i++)
    {
        snprintf(&buf[offset], 100 - offset, "axis%d tary num is:%d     ", i, trayNum[i]);
        offset = strlen(buf);
        if(offset > 100)
        {
            return;
        }
    }
    ESP_LOGI(TAG, "%.*s", strlen(buf), buf);
}

bool UpSensorLogFlag = 0;

void setOpenUpSeneorLog(void *n)
{
    UpSensorLogFlag = true;
}

void setCloseUpSeneorLog(void *n)
{
    UpSensorLogFlag = false;
}

PosCheckState_t PosCheckState[AxisNum] = {InitialState};

void getPosState(void* n)
{
    ESP_LOGI(TAG, "now pos state is:%d", PosCheckState[Axis1]);
}

void PosSensor_task(void *pvParameters) 
{
    uint16_t result;
    uint8_t channel;
    bool alert;
    MotorPosState_t MotorPosState;

    PosCheckState_t lastPosCheckState[AxisNum] = {PosCheckStateNum};
    PosHandCmd_t HandIns;

    MyCMDRegisterIns(&CmdHandle, InsSet, "motorsleep", setMotorSleep);
    MyCMDRegisterIns(&CmdHandle, InsSet, "motorsb", setMotorSearchBottom);
    MyCMDRegisterIns(&CmdHandle, InsSet, "motorrun", setMotorRun);
    MyCMDRegisterIns(&CmdHandle, InsGet, "tarynum", getTaryNum);
    MyCMDRegisterIns(&CmdHandle, InsSet, "openlog", setOpenUpSeneorLog);
    MyCMDRegisterIns(&CmdHandle, InsSet, "closelog", setCloseUpSeneorLog);
    MyCMDRegisterIns(&CmdHandle, InsGet, "posstate", getPosState);

    MotorCodeQueue = xQueueCreate(sizeof(MotorPosState_t), 20);
    MotorHandCtrlQueue = xQueueCreate(sizeof(PosHandCmd_t), 5);

    i2c_master_init();
    vTaskDelay(pdMS_TO_TICKS(1000));
    pac9535_init(PCA_INPUT, PCA_OUTPUT, PCA_LOW);
    for(uint8_t i = 0; i < AxisNum; i++)
    {
        pac9535_set_output(PAC_PORT1, (PCA_Pin_t)PosSenCtrlPin2RealPin[i], true);
    }
    uint8_t dev_addr = 0x22;
    ad799x_init(dev_addr);
    ad799x_set_channel(dev_addr, PosSenUpperChAll);
    taryZeroPos[0] = 5;
    for (;;) 
    {
        if(ulTaskNotifyTake(pdTRUE, portMAX_DELAY) != 0)
        {
            // ESP_LOGI(TAG, "pos task take notify time is:%lld", esp_timer_get_time()/1000);
            if(xQueueReceive(MotorHandCtrlQueue, &HandIns, 0) == pdPASS)
            {
                if(HandIns.PosCheckState == MotorSleep || HandIns.PosCheckState == HandSearchDownward || HandIns.PosCheckState == RunState)
                {
                    PosCheckState[HandIns.Axis] = HandIns.PosCheckState;
                }  
            }
            for(uint8_t i = 0; i < AxisNum; i++)
            {
                if(lastMotorPos[i] == currentMotorPos[i])
                {
                    MotorPosState.MotorState = MotorStop;
                }
                else
                {
                    ESP_LOGI(TAG, "state judge run ,last pos is%f    current pos is%f", lastMotorPos[MotorPosState.MotorAxis], currentMotorPos[MotorPosState.MotorAxis]);
                    lastMotorPos[i] = currentMotorPos[i];
                    MotorPosState.MotorState = MotorRunning;
                }

                switch (PosCheckState[i])
                {
                case InitialState:
                        PosCheckState[i] = SearchDownward;
                    break;
                case SearchDownward:
                    if (pac9535_get_input(PAC_PORT0, (PCA_Pin_t)PosSenDownPin2RealPin[i]) == BottomSensorTrigger)
                    {
                        //停止电机运动，留足时间记录在底部的坐标
                        MotorPosState.MotorRunState = MotorIdle;
                        MotorPosState.MotorAxis = i;
                        xQueueSend(MotorCodeQueue, &MotorPosState, portMAX_DELAY);
                        vTaskDelay(pdMS_TO_TICKS(500));
                        //底部坐标记录完成后开始探顶
                        SetTaryZeroPos(i);
                        PosCheckState[i] = SearchTop;
                    }
                    else
                    {
                        MotorPosState.MotorRunState = MotorDowmMove;
                        MotorPosState.MotorAxis = i;
                        xQueueSend(MotorCodeQueue, &MotorPosState, portMAX_DELAY);
                    }
                    break;
                case SearchTop:
                    ad799x_mode2_trigger_conversion(dev_addr, PosSenUpTran2RealPin[i]);
                    ad799x_read_conversion_result(dev_addr, &result, &channel, &alert);
                    if(UpSensorLogFlag)
                    {
                        ESP_LOGI(TAG, "Top sensor value is:%d", result);
                    }
                    if(result < UpSensirTH)
                    {
                        MotorPosState.MotorRunState = MotorIdle;
                        MotorPosState.MotorAxis = i;
                        xQueueSend(MotorCodeQueue, &MotorPosState, portMAX_DELAY);
                        PosCheckState[i] = RunState;
                        trayNum[i] = GetTrayNum(i);
                    }
                    else
                    {
                        MotorPosState.MotorRunState = MotorUpMove;
                        MotorPosState.MotorAxis = i;
                        xQueueSend(MotorCodeQueue, &MotorPosState, portMAX_DELAY);
                    }
                    break;
                case RunState:
                    ad799x_mode2_trigger_conversion(dev_addr, PosSenUpTran2RealPin[i]);
                    ad799x_read_conversion_result(dev_addr, &result, &channel, &alert);
                    if(UpSensorLogFlag)
                    {
                        ESP_LOGI(TAG, "Top sensor value is:%d", result);
                    }
                    if(result > UpSensirTH)
                    {
                        MotorPosState.MotorRunState = MotorUpMove;
                        MotorPosState.MotorAxis = i;
                        if(xQueueSend(MotorCodeQueue, &MotorPosState, portMAX_DELAY) != 1)
                        {
                            ESP_LOGI(TAG, "send queue failed");
                        }
                    }
                    else
                    {
                        MotorPosState.MotorRunState = MotorIdle;
                        MotorPosState.MotorAxis = i;
                        xQueueSend(MotorCodeQueue, &MotorPosState, portMAX_DELAY);
                        trayNum[i] = GetTrayNum(i);
                    }
                    break;
                case MotorSleep:
                    if(sleepFlag)
                    {
                        ESP_LOGI(TAG, "do motor sleep!!");
                        MotorPosState.MotorRunState = MotorIdle;
                        MotorPosState.MotorAxis = i;
                        xQueueSend(MotorCodeQueue, &MotorPosState, portMAX_DELAY);
                        sleepFlag = 0;
                    }
                    break;
                case HandSearchDownward:
                    if (pac9535_get_input(PAC_PORT0, (PCA_Pin_t)PosSenDownPin2RealPin[i]) == (!BottomSensorTrigger))
                    {
                        MotorPosState.MotorRunState = MotorDowmMove;
                        MotorPosState.MotorAxis = i;
                        xQueueSend(MotorCodeQueue, &MotorPosState, portMAX_DELAY);
                    }
                    // if (pac9535_get_input(PAC_PORT0, (PCA_Pin_t)PosSenDownPin2RealPin[i]) == BottomSensorTrigger)
                    else
                    {
                        sleepFlag = 1;
                        SetTaryZeroPos(i);
                        PosCheckState[i] = MotorSleep;
                    }
                    break;
                default:
                    ESP_LOGI(TAG, "pos check fun error!!");
                    break;
                }
                lastPosCheckState[i] = PosCheckState[i];
                // vTaskDelay(pdMS_TO_TICKS(50));
            }
        }
    }
    vTaskDelete(NULL);
}

static uint8_t GetTrayNum(MotorAxis_t Axis)
{
    double intpart;
    double height = TrayTravel - (taryZeroPos[Axis] - getMotorPos(Axis));
    if(height > 0)
    {
        return height / TrayHeight + ((modf((height / TrayHeight), &intpart) > 0.85) ? 1 : 0);
    }
    return 0xff;
}

static void SetTaryZeroPos(MotorAxis_t Axis)
{
    ESP_LOGI(TAG, "set zero pos\r\n\r\n");
    taryZeroPos[Axis] = getMotorPos(Axis);
}

double getZeroPos(MotorAxis_t Axis)
{
    return taryZeroPos[Axis];
}

double getMotorPos(MotorAxis_t Axis)
{
    return currentMotorPos[Axis];
}